#include "jump_control.h"
#include "leg_control.h"
#include "pid_balence.h"
uint8 jump_flag_today = 1;

void state_control(int count);


const struct jump_state jump_config[] = 
{
    {0,21,state_control},
    {21,43,state_control},
    {43,71,state_control},
    {71,91,state_control},
    {91,107,state_control},
};

struct jump_control_S jump =
{
    .flag = 0,
    .time = 0,
    .size = sizeof( jump_config ) / sizeof(struct jump_state),
};


void state_control(int count)
{
    
    
    switch (count)
    {
    case 0:
        pid_balence.left_leg.angle = 0;
        pid_balence.left_leg.length = 2.7;
        pid_balence.right_leg.angle = 0;
        pid_balence.right_leg.length = 2.7;
        break;

    case 1:
        

        pid_balence.left_leg.angle = 0;
        pid_balence.left_leg.length = 14.5;
        pid_balence.right_leg.angle = 0;
        pid_balence.right_leg.length = 14.5;
        pid_balence.mode.plane = plane_jump;
        pid_balence.mode.steering = steering_jump;
        break;
    case 2:

        pid_balence.left_leg.angle = 0;
        pid_balence.left_leg.length = 2.7;
        pid_balence.right_leg.angle = 0;
        pid_balence.right_leg.length = 2.7;

        break;
    case 3:

        pid_balence.left_leg.angle = 0;
        pid_balence.left_leg.length = 5.5;
        pid_balence.right_leg.angle = 0;
        pid_balence.right_leg.length = 5.5;


        break;
    case 4:
        pid_balence.left_leg.angle = 0;
        pid_balence.left_leg.length = 5.5- (jump.time- jump_config[4].min)*2/(jump_config[4].max - jump_config[4].min);
        pid_balence.right_leg.angle = 0;
        pid_balence.right_leg.length = 5.5- (jump.time- jump_config[4].min)*2/(jump_config[4].max - jump_config[4].min);
        pid_balence.mode.plane = plane_normal;
        pid_balence.mode.steering = steering_normal;
        break;

    default:
        break;
    }
}

void jump_control(void)
{
    if(jump.flag == 1)
    {
        jump.time++;
        if(jump.time < jump_config[jump.size - 1].max)
        {
            for(int i = 0; i<jump.size; i++)
            {

                if(jump.time >= jump_config[i].min && jump.time <= jump_config[i].max)
                {
                    jump_config[i].jump_step_jump(i);
                    break;
                }
            }
        }
        else
        {
            jump.flag = 0;
            jump.time = 0;
        }

    }
}
